/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvWorld/TrajSrc/side_traj.cpp
 * @Description  : 实现了侧移轨迹的生成与管理，包括创建轨迹、设置轮子移动方向、计算速度、投影姿态等功能。 
 *                 主要用于根据给定的起点、终点、航向角等信息生成车辆的侧移轨迹，并提供相关的计算与处理方法。   
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 17:25:09
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include <math.h> 
#include "trajectory.h"

/////////////////////////////////////////////////////////////////////////////
//   Implementation of the class.

/**
 * @brief 创建侧移轨迹
 *
 * 根据给定的起点、终点、航向角、起始位置和终止位置生成侧移轨迹。
 *
 * @param start_point 起点位置
 * @param end_point   终点位置
 * @param heading_angle 航向角
 * @param from        起始位置（相对于轨迹线的长度）
 * @param to          终止位置（相对于轨迹线的长度）
 */
void SideTrajectory::CreateTrajectory(const Point& start_point, const Point& end_point,
                                      const Angle& heading_angle, float from, float to) 
{
    type_ = (short)TrajType::SIDE_TRAJ;
    line_ = Line(start_point, end_point);
    float length = line_.Length();
    if (from > length) from = length;
    if (to < 0 || to > length) to = length;
    from_ = from;
    to_ = to;
    range_ = std::fabs(to - from);
    heading_angle_ = heading_angle;
    steer_angle_ = line_.SlantAngle() - heading_angle_;
    shift_angle_ = steer_angle_;
    SetWheelDir();
}

/**
 * @brief 设置车轮移动方向和侧向移动方向
 *
 * 根据转向角度设置车轮移动方向和侧向移动方向。
 *
 * 如果转向角度小于90度，则车轮向前移动，侧向向左移动；
 * 如果转向角度在90到180度之间，则车轮向后移动，并将转向角度取反，侧向向左移动；
 * 如果转向角度在180到270度之间，则车轮向后移动，并将转向角度取反，侧向向右移动；
 * 如果转向角度大于270度，则车轮向前移动，侧向向右移动。
 */
void SideTrajectory::SetWheelDir()
{
	// The rudder angle here is calculated by increasing it anticlockwise,
	// which is the same as that of the front of the car. It is 0 degree rudder angle.
	// Turning it anticlockwise to the left is 90 degree rudder angle, and the wheel turns forward, which is the positive direction (forward direction) of moving left, that is, hard sideward
	// Because the rudder can only be steered in the range of 90 0 -90. Note: the angle is automatically converted to 0-2i (360) in the cangle constructor
    if (LessDegree(steer_angle_, 90.0f)) {
        wheel_move_dir_ = MoveDirTag::Forward;
        side_move_dir_ = MoveDirTag::Leftward;
    } else if (LessDegree(steer_angle_, 180.0f)) {
        wheel_move_dir_ = MoveDirTag::Backward;
        steer_angle_ = !steer_angle_;
        side_move_dir_ = MoveDirTag::Leftward;
    } else if (LessDegree(steer_angle_, 270.0f)) {
        wheel_move_dir_ = MoveDirTag::Backward;
        steer_angle_ = !steer_angle_;
        side_move_dir_ = MoveDirTag::Rightward;
    } else {
        wheel_move_dir_ = MoveDirTag::Forward;
        side_move_dir_ = MoveDirTag::Rightward;
    }
    DEBUG_WARN_OUT("SideTraj: WheelMoveDir=" << (int)wheel_move_dir_.move_dir_tag_ << ",SideMoveDir=" << (int)side_move_dir_.move_dir_tag_);
	// here m_angSteer is[0,2pi]
	// real use anglesteer is [-pi,pi]
}

/**
 * @brief 设置相对进度
 *
 * 根据给定的速率和当前长度，更新轨迹的相对进度。
 *
 * @param rate 速率，取值范围在0到1之间
 * @param current_length 当前长度
 */
void SideTrajectory::SetRelProgress(float rate, float current_length) 
{
    progress_rate_ = rate;
    current_point_ = line_.TrajFun(current_length);
}

/**
 * @brief 获取侧轨迹的速度函数
 *
 * 根据车轮移动方向和转向角度计算线性速度、x轴速度、y轴速度和角速度，并返回速度对象。
 *
 * @return 返回速度对象
 */
Velocity& SideTrajectory::VelocityFun() 
{
    velocity_.linear_ = (wheel_move_dir_ == MoveDirTag::Forward) ? progress_rate_ : -progress_rate_;
    velocity_.vel_x_ = cos(steer_angle_) * velocity_.linear_;
    velocity_.vel_y_ = sin(steer_angle_) * velocity_.linear_;
    velocity_.angular_ = 0.0f;
    return velocity_;
}

/**
 * @brief 根据虚拟速度和转向角度计算横向轨迹的速度
 *
 * 根据给定的虚拟速度和转向角度，计算横向轨迹的速度。
 *
 * @param virtual_velocity 虚拟速度
 * @param steer_direction 转向方向，函数将更新此参数以反映转向方向
 *
 * @return 返回计算得到的速度结构体
 */
Velocity SideTrajectory::VelocityForSteerAngle(const float& virtual_velocity, int& steer_direction) 
{
    Velocity vel;
    vel.linear_ = (wheel_move_dir_ == MoveDirTag::Forward) ? virtual_velocity : -virtual_velocity;
    vel.vel_x_ = cos(steer_angle_) * vel.linear_;
    vel.vel_y_ = sin(steer_angle_) * vel.linear_;
    vel.angular_ = 0.0f;
    if (fabs(vel.vel_x_) < 1e-7f) {
        DEBUG_WARN_OUT("side traj: vx=" << vel.vel_x_ << ", force set 0.0");
        vel.vel_x_ = 0;
    }
    steer_direction = steer_angle_.NormAngle2() > 0.0f ? (int)SteerDirReport::SteerDirPositie : (int)SteerDirReport::SteerDirOpposite;
    DEBUG_WARN_OUT("SideTraj: VelocityForSteerAngle: virtual_velocity=" << virtual_velocity << ", steer_angle(degree)=" << steer_angle_.Degree()
                                                                        << ", steer_direction=" << steer_direction
                                                                        << ", vel.vel_x_=" << vel.vel_x_ << ", vel.vel_y_=" << vel.vel_y_ << ", vel.angular_=" << vel.angular_);
    return vel;
}

/**
 * @brief 计算当前朝向角度与直线切线角度的差值
 *
 * 计算当前朝向角度与直线切线角度的差值，并返回该差值。
 *
 * @return Angle 当前朝向角度与直线切线角度的差值
 */
Angle SideTrajectory::HeadToTangent() 
{
    Angle angle = heading_angle_ - line_.SlantAngle();
    DEBUG_OUT("SideTraj: HeadToTangent: heading_angle(degree)=" << heading_angle_.Degree()
              << ", tangentAngle(degree)=" << line_.SlantAngle().Degree()
              << ", angle(degree)=" << angle.Degree());
    return angle;
}

/**
 * @brief 判断是否可以进行转向预留
 *
 * 判断当前车辆的转向角度是否处于可以进行转向预留的范围内。
 *
 * @return 如果可以进行转向预留，则返回 true；否则返回 false。
 */
bool SideTrajectory::CanSteerReserve() 
{
    bool left = (!steer_angle_) > (PI / 2 + PI / 32);
    bool right = (!steer_angle_) < ((3 * PI) / 2 - PI / 32);
    return !(left && right);
}

/**
 * @brief 反转转向操作
 *
 * 根据当前状态，反转车轮移动方向和转向角度。
 *
 * 如果当前状态允许反转转向操作，则执行反转操作，并输出反转结果。
 */
void SideTrajectory::ReserveSteer() 
{
    if (CanSteerReserve()) {
        wheel_move_dir_ = (wheel_move_dir_ == MoveDirTag::Forward) ? MoveDirTag::Backward : MoveDirTag::Forward;
        steer_angle_ = !steer_angle_;
        DEBUG_OUT("ReserveSteer OK: WheelMoveDir=" << static_cast<int>(wheel_move_dir_.move_dir_tag_) << ", steer_angle(degree)=" << steer_angle_.Degree());
    }
}

/**
 * @brief 将给定姿态投影到轨迹上，并填充投影数据
 *
 * 将给定的姿态投影到轨迹上，并填充投影数据。如果姿态位于轨迹的起点或终点附近，或者位于轨迹的延长线上，
 * 则将投影类型设置为“内部”。如果姿态位于轨迹的延长线外，则根据姿态与轨迹起点的相对位置设置投影类型为“外部起点”或“外部终点”，
 * 并计算相应的投影坐标和相对进度。
 *
 * @param posture 姿态对象，包含位置和朝向信息
 * @param data 投影数据对象，用于存储投影结果
 * @param init_posture 是否初始化姿态（未使用）
 *
 * @return 如果投影成功，则返回true；否则返回false
 */
bool SideTrajectory::ProjectPosture(Posture& posture, ProjectData* data, bool init_posture) 
{
    (void)(init_posture);

    Angle angle_start_to, angle_end_to;
    float length_start_to;
    bool ret;
    // 计算姿态角度与航向角度的差值
    Angle angle_diff = posture.GetAngle() - heading_angle_;
    // 定义模糊节点长度
    float fuzzy_node_length = MAX_DIS_NEAR_POINT;

    // 初始化投影数据
    data->y_ = 0;
    data->curvature_ = 0;
    data->theta_ = angle_diff.radian_;
    data->tangent_ = heading_angle_.radian_;
    // 检查theta是否为NaN
    if (std::isnan(data->theta_)) {
        // 如果theta为NaN，则重置数据并输出错误信息
        data->x_ = data->y_ = data->theta_ = 0.0f;
        DEBUG_ERROR_OUT("Error: SideTraj: theta is NaN");
        return false;
    }

    // 获取姿态的实际点
    Point& point_actual = posture.GetPntObject();

    // 检查实际点是否与直线的起点不同
    if (line_.GetStartPoint() != point_actual) {
        // 计算从直线起点到实际点的线段和角度
        Line line_start_to_actual(line_.GetStartPoint(), point_actual);
        length_start_to = line_start_to_actual.Length();
        angle_start_to = line_start_to_actual.SlantAngle() - line_.SlantAngle();
        // 如果长度小于模糊节点长度，则判断为内部投影
        if (length_start_to < fuzzy_node_length) {
            data->type_ = ProjectData::ProjectionType::type_inside;
            DEBUG_OUT("#1 side project: inside; near start point; length_start_to=" << length_start_to);
        }
    } else {
        // 如果实际点与直线的起点相同，则判断为内部投影
        data->type_ = ProjectData::ProjectionType::type_inside;
        data->x_ = 0;
        data->relative_progress_ = 0;
        DEBUG_OUT("#2 side project: inside; location start point");
        return true;
    }

    // 检查实际点是否与直线的终点不同
    if (line_.GetEndPoint() != point_actual) {
        // 计算从直线终点到实际点的线段和角度
        Line line_end_to_actual(line_.GetEndPoint(), point_actual);
        angle_end_to = !line_.SlantAngle() - line_end_to_actual.SlantAngle();
        // 如果长度小于模糊节点长度，则判断为内部投影
        if (line_end_to_actual.Length() < fuzzy_node_length) {
            data->type_ = ProjectData::ProjectionType::type_inside;
            DEBUG_OUT("#3 side project: inside; near end point");
        }
    } else {
        // 如果实际点与直线的终点相同，则判断为内部投影
        data->type_ = ProjectData::ProjectionType::type_inside;
        data->x_ = 0;
        data->relative_progress_ = length_start_to - from_;
        DEBUG_OUT("#5 side project: inside; location end point");
        return true;
    }

    // 判断角度是否在90度到270度之间
    if (angle_start_to.Degree() > 90.0f && angle_start_to.Degree() < 270.0f) {
        // 如果是，则判断为外部起点投影
        data->type_ = ProjectData::ProjectionType::type_outside_start;
        data->x_ = -length_start_to * sin(angle_start_to);
        data->y_ = -std::fabs(length_start_to * cos(angle_start_to));
        data->relative_progress_ = 0;
        ret = false;
        DEBUG_ERROR_OUT("#6 error: side project: outside start, x=" << data->x_ << ", y=" << data->y_ << ", relProg=0");
    } else if (angle_end_to.Degree() > 90.0f && angle_end_to.Degree() < 270.0f) {
        // 如果是，则判断为外部终点投影
        data->type_ = ProjectData::ProjectionType::type_outside_end;
        data->x_ = -length_start_to * sin(angle_start_to);
        data->y_ = std::fabs(length_start_to * cos(angle_start_to)) - from_;
        data->relative_progress_ = data->y_;
        ret = false;
        DEBUG_ERROR_OUT("#7 error: side project: outside end, x=" << data->x_
                                                                   << ", y=" << data->y_
                                                                   << ", relProg=" << data->relative_progress_);
    } else {
        // 否则，判断为内部投影
        data->type_ = ProjectData::ProjectionType::type_inside;
        data->x_ = -length_start_to * sin(angle_start_to);
        data->relative_progress_ = length_start_to * cos(angle_start_to) - from_;
        data->y_ = data->relative_progress_;
        ret = true;
        DEBUG_ERROR_OUT("#8 ok: side project: inside, x=" << data->x_ << ", y=" << data->y_ << ", relProg=" << data->relative_progress_);
    }

    // 根据车轮移动方向调整x坐标
    if (WheelMoveDir() == MoveDirTag::Backward) {
        data->x_ = -data->x_;
    }
    return ret;
}
